#!/usr/bin/env python3

import numpy as np
from .ekf import EKF

class GpsFusion1D(EKF):
    def __init__(self):
        # Init data
        # Current accelerated velocity
        self.a = 0
        # Delta time
        self.delta_t = 1
        # Init filter
        self.state_dims_ = 2        # Dimensionalities for state-space.
        self.observe_dims = 1       # Dimensionalities for observation-space.
        self.x0 = [[100],[0]]       # Start position. Usage: np.array([[95],[1.0]]).
        self.p0 = [[0, 0], [0, 0]]  # Covariance Matrix for Start position. Usage: np.array(p0).
        self.q = [[5,0],[0,0.5]]    # Covariance Matrix for Prediction-Model. Usage: np.array(q) .
        self.r = [[20]]             # Covariance Matrix for Observation-Model. Usage: 
        super().__init__(self.state_dims_,
                        self.observe_dims,
                        self.x0,
                        self.p0,
                        self.q,
                        self.r)

    def f(self, x):
        """
        The original model is $\hat x_t = F(x_{t-1},u_{t-1})$. For EKF, it is 
        simplified as $\hat x_t = F x_{t-1} + B u_{t-1}$.
        """
        # $F_t = F_J(x)|_{x=x_{t-1}}$
        F_t = self.__F(x, self.delta_t)
        B_t = self.__B(self.delta_t)
        # $\hat x_t = F * x_{t-1} + B * u_{t-1}$
        # For EKF, Here should be $\hat x_t = F(x_{t-1}, u_{t-1})$.
        x = F_t @ x + B_t * self.a
        return x,F_t

    def h(self, x):
        """
        The original model is $z_t = H(x_{t})$.
        """
        H_t = self.__H(x)
        # For EKF, Here should be $z_t = H(x_t)$
        h = H_t @ x
        return h,H_t
    
    def __F(self,x,delta_t):
        """
        F is a matrix which is used to transform $x_{t-1}$ to $x_t$. i.e: 
        $x_t = F x_{t-1}$. For linear model, F is a const matrix, but it's a 
        Jacobian Matrix for non-linear model.
        """
        mat = [[1.0, delta_t],[0.0, 1.0]]
        return np.array(mat)

    def __B(self, delta_t):
        """
        B is a matrix which is used to represent control-vector. i.e: $B u_{t-1}$
        """
        mat = [[0.5*(delta_t**2)], [delta_t]]
        return np.array(mat)

    def __H(self, x):
        """
        H is a matrix which is used to transform State-Space to Observation-Space.
        i.e: Z=H@X .For linear model, H is a const matrix, but it's a Jacobian 
        Matrix for non-linear model.
        """
        mat = [[1, 0]]
        return np.array(mat)
